package edu.cmu.hcii.modalnodes;

import java.util.logging.Logger;

import processing.core.PApplet;
import processing.core.PVector;
import SimpleOpenNI.SimpleOpenNI;

public class KinectUtilities {
	private final Logger log = Logger.getLogger(KinectUtilities.class.getName());

	/**
	 * Draws the skeleton for the given user id
	 * 
	 * @param userId the id of the user to draw
	 */
	public static void drawSkeleton(PApplet parent, SimpleOpenNI ni, int userId) {
		parent.pushStyle();
		
		parent.strokeWeight(10);
		
		ni.drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK);
		 
		ni.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER);
		ni.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW);
		ni.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND);
		 
		ni.drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER);
		ni.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW);
		ni.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND);
		 
		ni.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
		ni.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO);
		 
		ni.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP);
		ni.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);
		ni.drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);
		 
		ni.drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP);
		ni.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
		ni.drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);
		
		parent.popStyle();
	}
	
	/**
	 * Return the position of the given joint for the given user.
	 * 
	 * TODO this isn't a completely accurate projection position.
	 * 		Need to actually calibrate based on the projected pane.
	 * 
	 * @param ni the openNI context
	 * @param userId the id of the user to check
	 * @param joint the joint to project
	 * @return
	 */
	public static PVector getProjectionJointPosition(SimpleOpenNI ni, int userId, int joint) {
		PVector jointPos = new PVector();
	    ni.getJointPositionSkeleton(userId, joint, jointPos);
	    PVector projectionPos = new PVector();
	    ni.convertRealWorldToProjective(jointPos,  projectionPos);
	    
	    projectionPos.x = (projectionPos.x / ni.depthWidth()) * Sequencer.windowWidth;
	    projectionPos.y = (projectionPos.y / ni.depthHeight()) * Sequencer.windowHeight;
	    
	    return projectionPos;
	}
	
	/**
	 * Checks to see if the given user's left arm is making a right angle with 
	 * the hand, elbow, and shoulder as the three vertices.
	 * 
	 * TODO this does not take orientation into account.  Meaning, the hand can 
	 * be either above or below the shoulder.
	 * 
	 * @param ni the openNI context
	 * @param userId the id of the user to check
	 * @return if the left arm forms a right angle
	 */
	public static boolean armRightAngle(SimpleOpenNI ni, int userId) {
		PVector lHand = new PVector();
		PVector lElbow = new PVector();
		PVector lShoulder = new PVector();
		
		ni.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HAND, lHand);
		ni.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, lElbow);
		ni.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, lShoulder);

		return isHorizontal(lElbow, lShoulder, 100) && isVertical(lHand, lElbow, 80);
	}
	
	/**
	 * Checks to see if the given user is making a "time out" T shape with their
	 * arms.
	 * 
	 * @param ni the openNI context
	 * @param userId the id of the user to check
	 * @return if arms are making a T shape
	 */
	public static boolean timeOut(SimpleOpenNI ni, int userId) {
		PVector rHand = new PVector();
		PVector rElbow = new PVector();
		PVector lHand = new PVector();
		PVector lElbow = new PVector();
		
		ni.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND, rHand);
		ni.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, rElbow);
		ni.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HAND, lHand);
		ni.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, lElbow);
		
		//TODO this needs to be further refined, but it works.
		if (isHorizontal(lHand, lElbow, 50)) {
			return isVertical(rHand, rElbow, 50) && xInOrder(lElbow, rHand, lHand) && isHorizontal(lHand, rHand, 150);
		} else if (isHorizontal(rHand, rElbow, 50)) {
			return isVertical(lHand, lElbow, 50) && xInOrder(rHand, lHand, rElbow) && isHorizontal(lHand, rHand, 150);
		}
		
		return false;
	}
	
	/**
	 * Checks if the given user has their hands together above their head.
	 * 
	 * @param ni the openNI context
	 * @param userId the id of the user to check
	 * @return if hands are together above the head
	 */
	public static boolean handsTogether(SimpleOpenNI ni, int userId) {
		PVector rHand = new PVector();
		PVector lHand = new PVector();
		PVector head = new PVector();
		
		ni.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_RIGHT_HAND, rHand);
		ni.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_LEFT_HAND, lHand);
		ni.getJointPositionSkeleton(userId, SimpleOpenNI.SKEL_HEAD, head);
		
		//The real world y's are opposite of screen position
		return rHand.y >= head.y && lHand.y >= head.y && Math.abs(rHand.x - lHand.x) <= 100;
	}
	
	/**
	 * Returns if the two given vectors form a horizontal line within the 
	 * given error.
	 * 
	 * @param p1 the first point to check
	 * @param p2 the second point to check
	 * @param error the amount of error (distance) from a perfect horizontal
	 * @return if the two vectors form a horizontal line
	 */
	private static boolean isHorizontal(PVector p1, PVector p2, int error) {
		return Math.abs(p1.y - p2.y) <= error;
	}
	
	/**
	 * Returns if the two given vectors form a vertical line within the 
	 * given error.
	 * 
	 * @param p1 the first point to check
	 * @param p2 the second point to check
	 * @param error the amount of error (distance) from a perfect vertical
	 * @return if the two vectors form a vertical line
	 */
	private static boolean isVertical(PVector p1, PVector p2, int error) {
		return Math.abs(p1.x - p2.x) <= error ;
	}
	
	/**
	 * Checks if the given vectors occur in order based on their x values.
	 * 
	 * @param left the expected left-most vector
	 * @param center the expected center vector
	 * @param right the expected right-most vector
	 * @return if the three vectors are indeed in order
	 */
	private static boolean xInOrder(PVector left, PVector center, PVector right) {
		return left.x <= center.x && center.x <= right.x;
	}
	
	/**
	 * Checks if the given vectors occur in order based on their y values.
	 * These values are assumed to be screen position, so 0 == top.
	 * 
	 * @param top the expected top-most vector
	 * @param center the expected center vector
	 * @param bottom the expected bottom-most vector
	 * @return if the three vectors are indeed in order
	 */
	private static boolean yInOrder(PVector top, PVector center, PVector bottom) {
		return top.y <= center.y && center.y <= bottom.y;
	}
}
